/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 *
 * \file
 * \brief MaCI - JointGroupCtrl interface type definitions
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */
#ifndef _MACI_INTERFACE_JOINTGROUPCTRLTYPES_HPP_
#define _MACI_INTERFACE_JOINTGROUPCTRLTYPES_HPP_
#include <inttypes.h>
#include <string.h>
#include <stdlib.h>
#include <string>
#include "devicePosition.hpp"
namespace MaCI {

  /** MaCI - JointGroupCtrl Interface
   *
   * This namespace contains all the methods and types related to
   * the MaCI - JointGroupCtrl Interface
   *
   */
  namespace JointGroupCtrl{

    /**
     * Type identifiers inside BinBag frame. Used for dynamic encoding/decoding
     * of message frames
     *
     */
    enum EJointGroupCtrlType { 
      KTypeUnknown                   = -1,  ///< Unknown type
      KTypeCommand                   = 1,   ///< Contains a Command (TCommand)
      KTypeJointSpeed                = 2,   ///< Contains a Joint Speed (TJointSpeed)
      KTypeJointPosition             = 3,   ///< Contains a Joint Position (TJointPosition)
      KTypeJointInformatioHeader     = 4,   ///< Contains a Joint information header (TJointInformationHeader)
      KTypeJointInformation          = 5,   ///< Contains a Joint information (TJointInformation_Payload)
      KTypeJointNumber               = 6,   ///< Contains a Joint index (TJointNumber)
      KTypeJointGroupStatus          = 7, 
      KTypeJointForce                = 8,   ///< Contains a Joint Force (TJointForce)
      KTypeEventType                 = 9
      
    };
    /**
     * Commands available (Commands are paired with appropriate following dataelements)
     *
     */
    enum EJointGroupCtrlCommand {
      KCommandUnknown = -1,             ///< Unknown type
      KCommandGetSpeed = 1,             ///< Get Speed of one joint
      KCommandGetPosition = 2,          ///< Get Position of one joint
      KCommandGetJointInformation = 3,  ///< Get joint information
      KCommandGetJointGroupStatus = 4,  ///< Get joint group status
      KReplyGetSpeed              = 5,  ///< Reply for Get Speed query
      KReplyGetPosition           = 6,  ///< Reply for Get Position query
      KReplyGetJointInformation   = 7,  ///< Reply for Get Joint Information query
      KReplyGetJointGroupStatus   = 8,  ///< Reply for Get Joint Group Status query
      KCommandGetForce            = 9,  ///< Get Force
      KReplyGetForce              = 10, ///< Reply for Get Force query
      KCommandGetGroupPositions   = 11, ///< Get position from all joints
      KReplyGetGroupPositions     = 12, ///< Reply for Get position from all joint
      KCommandGetGroupSpeeds      = 13, ///< Get position from all joints
      KReplyGetGroupSpeeds        = 14, ///< Reply for Get position from all joint
      KCommandGetGroupForces      = 15, ///< Get position from all joints
      KReplyGetGroupForces        = 16, ///< Reply for Get position from all joint
      KCommandSetGroupSpeeds      = 17,
      KCommandSetGroupPositions   = 18,
      KCommandSetGroupForces      = 10,
      KCommandGetEventTypes       = 20,
      KReplyGetEventTypes         = 21,
      KCommandEvent               = 22,
      KCommandUpdateState         = 23  ///< Command to use for new clients to have the state of the
                                        ///  joint group when connecting to the service. Then the server
                                        ///  side should send all event types that it provides.

 
   };

    enum EJointControlType{
      KJointControlAngular = 1,       ///< The joint is controlled by angle units. SetSpeed with [rad/s] and SetPosition with [rad]
      KJointControlLinear = 2,         ///< The joint is controlled by length units. SetSpeed with [m/s] and SetPosition with [m]
      KJointControlServo = 3          ///< The joint is a hobby servo. SetPosition with PWM pulse width in micro seconds (typical range 1000-2000) 
    };

    enum EJointGroupStatus{
      KJointGroupStatusOpen = 0,   ///< Joint group is open, ready to take commands
      KJointGroupStatusReset = 1,           ///< Joint group is initializing
      KJointGroupStatusClosed = 2,         ///< Joint group is closed
      KJointGroupStatusUnimplemented = 3   ///< Joint group status query is not implemented
    };

    /**
     * Defines which types are sent continuosly.
     * Basically  KTypeJointSpeed,KTypeJointPosition,KTypeJointForce can be sent as an event
     *  
     */
    struct TEventTypes
    {
      TEventTypes(const unsigned int aContinuousTypes = 0)
        : continuousTypes(aContinuousTypes){}

      void SetEventType(EJointGroupCtrlType aType){
        continuousTypes |= 1<<aType;

      }
      void UnsetEventType(EJointGroupCtrlType aType){
        uint32_t mask = 1<<aType;
        mask = ~mask;
        continuousTypes &= mask;

      }

      bool IsSet(EJointGroupCtrlType aType){
        uint32_t temp = continuousTypes;
        uint32_t mask = 1<<aType;
        temp &=mask;
        if(temp != 0){
          return true;
        }else{
          return false;
        }

      }

      uint32_t continuousTypes;    ///< continuous types, saved in bits. shifted by proper EJointGroupCtrlType


    };    

    
    /**
     * Struct for joint speed command container.
     * 
     * 
     */
    struct TJointSpeed{
      TJointSpeed(float aSpeed = 0.0, unsigned int aJointNumber=0)
	:speed(aSpeed),
	 jointNumber(aJointNumber){}
      
      float speed;                  ///< Speed to set [rad/s or m/s]
      uint32_t jointNumber;         ///< Joint index 
    };
    
   
    
    /**
     * Struct for joint position command container
     */
    struct TJointPosition{
      TJointPosition(float aPosition= 0.0, unsigned int aJointNumber= 0)
	:position(aPosition),
	 jointNumber(aJointNumber){}

      float position;               ///< Position to set [rad or m]
      uint32_t jointNumber;        ///< Joint Index
    };

    /**
     * Struct for joint force command container
     */
    struct TJointForce{
      TJointForce(float aForce= 0.0, unsigned int aJointNumber= 0)
	:force(aForce),
	 jointNumber(aJointNumber){}

      float force;                 ///< ForcePosition to set [N]
      uint32_t jointNumber;        ///< Joint Index
    };

    /**
     * Struct for joint number container
     */
    struct TJointNumber{
      TJointNumber(unsigned int aJointNumber= 0):
	number(aJointNumber){}
      uint32_t number;            ///< Joint index
    };

    /**
     * Structure for defining Denavit Hartenberg Parameters
     */
    struct TDenavitHartenberParameter{
      TDenavitHartenberParameter(float aA = 0.0,
                                 float aAlpha = 0.0,
                                 float aD= 0.0,
                                 float aTheta= 0.0)
        :a(aA),
         alpha(aAlpha),
         d(aD),
         theta(aTheta){}
      
      float a;             ///< The distance between Z_i to Z_i+1 measured along X_i
      float alpha;         ///< The angle from Z_i to Z_i+1 measured about X_i
      float d;             ///< The distance from X_i-1 to X_i measured along Z_i
      float theta;         ///< The distance from X_i-1 to X_i measured about Z_i
    };


    /**
     * Struct for defining one joint. It is used for sending the 
     * Data from server to client. 
     * NOTE: remember to set the name of the joint after constructing it.
     *       name can be set with SetName-function
     */
    struct TJointInformation_Payload{
      TJointInformation_Payload(unsigned int aJointNumber= 0,
				EJointControlType aType =KJointControlAngular,
				int aCanControlSpeed= 0,
				float aMaxSpeed = 0.0,
				int aCanControlPosition = 0,
				float aMinPosition = 0.0,
				float aMaxPosition = 0.0,
                                int aCanControlForce = 0,
                                float aMinForce = 0.0,
                                float aMaxForce = 0.0,
                                TDenavitHartenberParameter aDHParams = TDenavitHartenberParameter() )
	: name(),
          jointNumber(aJointNumber),
          jointControlType(aType),
          canControlSpeed(aCanControlSpeed),
          maxSpeed(aMaxSpeed),
          canControlPosition(aCanControlPosition),
          minPosition(aMinPosition),
          maxPosition(aMaxPosition),
          canControlForce(aCanControlForce),
          minForce(aMinForce),
          maxForce(aMaxForce),
          dHParams(aDHParams)
      {}
       
      /**
       * Sets name to the joint.
       * NOTE: max name length is 64 characters!
       *
       * @param aName     Name of the joint
       */
      void SetName(std::string aName)
      {
	memset(name, '\0', 64 );
	unsigned int length = aName.length();
	aName.copy(name,length);
      }
      
      
      char name[64];               ///< Name of the joint
      uint32_t jointNumber;        ///< Index of the joint
      uint32_t jointControlType;   ///< Control type of the joint(angular or linear)
      
      uint32_t canControlSpeed;    ///< Tells if the joint can be controlled by speed commands(0 for no, 1 for yes)
      float maxSpeed;              ///< Maximum speed of the joint [rad/s] or [m/s] depending of the control type
      
      uint32_t canControlPosition; ///< Tells if the joint can be controlled by position commands(0 for no, 1 for yes)
      float minPosition;           ///< Minimum position of the joint [rad] or [m] depending of the control type
      float maxPosition;           ///< Maximum position of the joint [rad] or [m] depending of the control type

      int canControlForce;         ///< Tells if the joint can be controlled by force commands(0 for no, 1 for yes)
      float minForce;              ///< Minimum control force of the joint [N]
      float maxForce;              ///< Maximum control force of the joint [N]
      TDenavitHartenberParameter dHParams; ///< Denavit-Hartenberg params
    };
    
    /**
     * Header for jointInformatio_Payload, to tell how many joint
     * are coming after this header.
     */
    struct TJointInformationHeader{
      TJointInformationHeader(unsigned int aNumberOfJoints= 0,
                              MaCI::Common::TDevicePosition aGroupPosition = MaCI::Common::TDevicePosition()):
	numberOfJoints(aNumberOfJoints),
        groupPosition(aGroupPosition){}
	
      uint32_t numberOfJoints;  ///< Tells how many jointinformation_payload struct are 
                                /// coming after this header
      MaCI::Common::TDevicePosition groupPosition; ///<Tells the position of the joint group. Defines also the base 
                                                   /// coordinate system of the denavit hartenbeg parameters
    };


      
    /**
     * Describes Commands for JointControl interface
     *
     */
    struct TCommand {
      TCommand(EJointGroupCtrlCommand aCmd = KCommandUnknown)
	: cmd(aCmd) {}
      
      uint32_t cmd; ///< Command \see EJointGroupCtrlCommand
    };

    /**
     * Struct for storing information to send reply messages
     */
    struct TReplyInfo {
      TReplyInfo(int aSenderId=0, int aMsgId=0) :
	senderId(aSenderId),
	msgId(aMsgId){}
      
      int senderId;  ///< Id of the sender
      int msgId;     ///< Id of the messages
    };

    /**
     * Struct for defining joint group status
     * \see EJointGroupStatus
     */
    struct TJointGroupStatus{
      TJointGroupStatus(EJointGroupStatus aStatus = KJointGroupStatusUnimplemented)
        :status(aStatus){}
      uint32_t status;   ///< Status \see EJointGroupStatus
    };

                          
  
  }
}

#endif  ///<_MACI_INTERFACE_JOINTGROUPCTRLTYPES_HPP_
